#include "waypoint_list.h"



waypoint_list_c::
waypoint_list_c()
{
	head = NULL;
	count = 0;
}

waypoint_list_c::
~waypoint_list_c()
{}


uint16_t 
waypoint_list_c::
insert_waypoint(uint16_t list_num, waypoint_t waypoint_item)
{
	waypoint_node_t *n, *pre, *post;
	n = (waypoint_node_t*)new(waypoint_node_t);
	n->waypoint = waypoint_item;
	n->seq = list_num;
	
	if (list_num == 0)
	{
		return 0;
	}
	
	if (head == NULL)
	{
		head = n;
		head->next = NULL;
		count = 1;
		printf("head=NULL\n");
		return count;
	}		
	else if (list_num > (count))
	{
		pre = head;
		while (pre->next != NULL)
		{
			pre = pre->next;
		}
		pre->next = n;		
		n->seq = ++count;
		n->next = NULL;
		return count;
	}	
	else 
	{
		post = head;
		while ((post->seq != list_num) && (post->next != NULL))
		{ 
			pre = post;
			post = post->next;
		}
		if (post->seq == list_num)
		{
			if (list_num == 1)
			{
				head = n;
			}
			else
			{
				pre->next = n;
			}
			n->next = post;					
			while (post->next != NULL)
			{
				post->seq++;
				post = post->next;
			}
			post->seq = ++count;
		}	

		return count;
	}
		
}


uint16_t 
waypoint_list_c::
delete_waypoint(uint16_t list_num)
{
	waypoint_node_t *o, *post;
	o = head;
	if (list_num > count)
	{
		return 0; 
	}
	else if (list_num == 1)
	{
		head = o->next;
		delete o;
		post = head;
		while (post->next != NULL)
		{
			post->seq--;
			post = post->next;
		}
		post->seq--;
		return --count;
	}
	else
	{
		while ((o->seq != list_num) && (o->next != NULL))
		{
			o = o->next;
		}
		if (o->next != NULL)
		{
			post = o->next;
			delete o;
			while (post->next != NULL)
			{
				post->seq--;
			}
			post->seq--;
		}
		return --count;		
	}
}


waypoint_t
waypoint_list_c::
get_waypoint(uint16_t list_num)
{
	waypoint_node_t *o;
	o = head;
	while((o->seq != list_num) && (o->next != NULL))
	{
		o = o->next;
	}
	return o->waypoint;	
}


waypoint_t
waypoint_list_c::
get_headpoint()
{
	return head->waypoint;
}

int
waypoint_list_c::
clear(void)
{	
	if (count == 0)
	{
		return 0;
	}
	else
	{
		waypoint_node_t *p, *q;
		p = head;
		for (int i = 1; i <= count; i++)
		{
			q = p->next;
			delete p;
			p = q;
		}
		head = NULL;
		count = 0;
		return 1;
	}	
}



